package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;

public class hiphop extends SimpleRobot {
    RobotDrive drive;
    Joystick leftStick;
    Timer timer = new Timer();
    Timer temptimer = new Timer();
    double max, beat, throt;
    int status;

    public hiphop(double maxspeed, double steptime, RobotDrive drive, Joystick leftStick){
        max = maxspeed;
        beat = steptime;
        this.leftStick = leftStick;
        this.drive = drive;
        status = 1;
        temptimer.start();
    }

    //main dance sequence
    public void dance(int seq){
        double twist, x;
        twist = 0;
        x = max;
        timer.start();
        //sequences: 1 = strafe LRLL, 2 = strafe RLRR, 3 = Twist LRLL, 4 = twist RLRR
        //if seq == 1, then max and twist are unchanged
        if (seq == 2 || seq == 4){
            x = -max;
        }
        if (seq == 3 || seq == 4){
            twist = twist + x*.75;
            x = 0;
        }

        //begin dance sequence
            checkuser();
            timer.reset();
            while(timer.get() <= beat && status == 1){
                accel(-x, beat, -twist);
                while(timer.get() <= beat*2/3){
                    drive.mecanumDrive_Cartesian(-x/throt,0,-twist/throt,0);
                }
                decel(-x, beat, -twist);
            }
            checkuser();
            timer.reset();
            while(timer.get() <= beat && status == 1){
                accel(x, beat, twist);
                while(timer.get() <= beat*2/3){
                    drive.mecanumDrive_Cartesian(x/throt,0,twist/throt,0);
                }
                decel(x, beat, twist);
            }
            checkuser();
            timer.reset();
            while(timer.get() <= beat && status == 1){
                accel(-x, beat, -twist);
                while(timer.get() <= beat*2/3){
                    drive.mecanumDrive_Cartesian(-x/throt,0,-twist/throt,0);
                }
                decel(-x, beat, -twist);
            }
            timer.reset();
            checkuser();
            while(timer.get() <= beat && status == 1){
                accel(-x, beat, -twist);
                while(timer.get() <= beat*2/3){
                    drive.mecanumDrive_Cartesian(-x/throt,0,-twist/throt,0);
                }
                decel(-x, beat, -twist);
            }
            status = 1;
    }

    //check for user command to stop by using button 9
    public void checkuser(){
        boolean laststate4, laststate6;
        laststate4 = laststate6 = false;
        //check for desired drive speed using the throttle bar
        throt = ((leftStick.getThrottle()+1)*3/2)+1;
        //stop the dance by holding down button 9
        if (leftStick.getRawButton(9)  == true){
            status = 0;
        }
        //speed up the dance steps using button 6
        if (leftStick.getRawButton(6) == true && laststate6 == false){
            beat = beat - 0.1;
        }
        laststate6 = leftStick.getRawButton(6);
        //slow down the dance steps using button 4
        if (leftStick.getRawButton(4) == true && laststate4 == false){
            beat = beat + 0.1;
        }
        laststate4 = leftStick.getRawButton(4);
    }
    //accelerates into each step
    public void accel(double x, double beat, double twist){
        temptimer.reset();
        beat = beat/3;
        while(temptimer.get() <= beat){
            if (temptimer.get() <= beat/3){
                drive.mecanumDrive_Cartesian(x*.25/throt,0,twist*.25/throt,0);
            }
            if (temptimer.get() > beat/3 && temptimer.get() <= beat*2/3){
                drive.mecanumDrive_Cartesian(x*.5/throt,0,twist*.5/throt,0);
            }
            if (temptimer.get() > beat*2/3 && temptimer.get() <= beat){
                drive.mecanumDrive_Cartesian(x*.75/throt,0,twist*.75/throt,0);
            }
        }
    }

    //decelerates out of each step
    public void decel(double x, double beat, double twist){
        temptimer.reset();
        beat = beat/3;
        while(temptimer.get() <= beat){
            if (temptimer.get() <= beat/3){
                drive.mecanumDrive_Cartesian(x*.75/throt,0,twist*.75/throt,0);
            }
            if (temptimer.get() > beat/3 && temptimer.get() <= beat*2/3){
                drive.mecanumDrive_Cartesian(x*.5/throt,0,twist*.5/throt,0);
            }
            if (temptimer.get() > beat*2/3 && temptimer.get() <= beat){
                drive.mecanumDrive_Cartesian(x*.25/throt,0,twist*.25/throt,0);
            }
        }
    }
}